#ifndef PCL_NO_PRECOMPILE
#define PCL_NO_PRECOMPILE
#endif
#include <iostream>
#include <memory>
#include <chrono>
#include <string>
#include <boost/make_shared.hpp>
#include <vector>
#include "rclcpp/rclcpp.hpp"

#include "sensor_msgs/msg/point_cloud2.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "std_msgs/msg/header.hpp"

#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>

#include "processcloud.hpp"
using namespace std;
class BerxelSubscriber : public rclcpp::Node
{
  public:
    BerxelSubscriber()
    : Node("my_berxerl")
    {
    
       using std::placeholders::_1;
        sub_berxel = this->create_subscription<sensor_msgs::msg::PointCloud2>("/berxel_cloudpoint", 
                10, std::bind(&BerxelSubscriber::topic_callback, this, std::placeholders::_1));
        publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/my_berxel_cloud", 10);
        pcc=new Process_Cloud();
    }
    private:
    void topic_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) 
    {
       int32_t size,i,j,k;
       int flag;
       pcl::PointCloud <pcl::PointXYZ> berxel_cloud;
       pcl::PointCloud <pcl::PointXYZ> conver_cloud;
      // pcl::PointCloud <pcl::PointXYZ>::Ptr input_cloud;
       pcl::PointCloud <pcl::PointXYZ>::Ptr output_cloud;
       pcl::PointXYZ point;
       //input_cloud=pcc->getinput_cloudptr();
       pcl::PointXYZ tempPoint(0.0, 0.0, 0.0);
       pcl::fromROSMsg(*msg, berxel_cloud);
       size= berxel_cloud.points.size();
       k=0;
       pcc->clear_input_point();
       for(int i=0;i<size;++i)
       {
        flag=0;
        if((berxel_cloud.points[i].x>50)||(berxel_cloud.points[i].x<-50))flag++;
        if((berxel_cloud.points[i].y>50)||(berxel_cloud.points[i].y<-50))flag++;
        if((berxel_cloud.points[i].z>50)||(berxel_cloud.points[i].z<-50))flag++;
        if(flag==0)
        {
          point.x=-berxel_cloud.points[i].z;
          point.y=berxel_cloud.points[i].x;
          point.z=-berxel_cloud.points[i].y;
          pcc->add_input_point(point);
          // if((k<220)&&(k>200))
          // {
          //   cout<<berxel_cloud.points[i].x<<" "<<berxel_cloud.points[i].y<<" "<<berxel_cloud.points[i].z<<endl;
            
          // }  
          // k++;
        }
        //if(k>300)break;
       }

      // pcc->vgrid(0.05f);
      // pcc->convert_cloud(tick);
       pcc->my_harris_3d();
       tick+=0.01;
       if(tick>1.0)tick=0;
       output_cloud=pcc->getoutput_cloudptr();
       k=pcc->input_cloud.points.size();
       size= output_cloud->points.size();
       RCLCPP_INFO(this->get_logger(), "I reveive: '%d'  '%d'",k,size);
         //发给rviz显示
        // if(trybb_cloud->points.size()>5)
        //     pcl::io::savePCDFileASCII ("cloud2.pcd", *trybb_cloud);
        //     RCLCPP_INFO(this->get_logger(), "I send: '%d'  zero=%6.2f ", trybb_cloud->points.size(),zero);
            sensor_msgs::msg::PointCloud2 mymsg;
            pcl::toROSMsg(*output_cloud, mymsg);
            mymsg.header.stamp =now();
            mymsg.header.frame_id="map";
            publisher_->publish(mymsg);    
    }
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_berxel;
    Process_Cloud *pcc;
    float tick=0;
};
int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<BerxelSubscriber>());
  
  rclcpp::shutdown();
  return 0;
}





